package ru.volgogradetzzz.ik
{
	import flash.display.Graphics;
	import ru.volgogradetzzz.math.VMatrix2x2;
	import ru.volgogradetzzz.math.VVector2;
	/**
	 * ...
	 * @author volgogradetzzz
	 */
	public class VCCDIKSolver 
	{
		private var _bones:Vector.<VBone>;
		private var _angleConstraints:Vector.<AngleConstraintsValues>;
		private var _numBones:uint;
		private var _numIterations:uint;
		
		public function VCCDIKSolver(position:VVector2, bones:Vector.<VBone>, angleConstraints:Vector.<AngleConstraintsValues>, numIterations:uint) 
		{
			if (angleConstraints.length < bones.length)
			{
				throw(new Error('Incorrect number of constraints.'));
			}
			
			_bones = bones;
			_angleConstraints = angleConstraints;
			_numIterations = numIterations;
			
			_numBones = _bones.length;
			
			for (var i:int = 0; i < _numBones; i++) 
			{
				_bones[i].position.add(position);
				position.add(_bones[i].direction);
				
				if (i > 0)
				{
					bones[i - 1].next = bones[i];
					bones[i].prev = bones[i - 1];
				}
			}
		}
		
		public function solve(target:VVector2):void
		{	
			for (var j:uint = 0; j < _numIterations; j++)
			{
				for (var i:int = _numBones - 1; i > -1; i--) 
				{
					var bone:VBone = _bones[i];
					
					var vecToTarget:VVector2 = target.subtractReturn(bone.position);
					var vecToEndPoint:VVector2 = bone.direction.clone();
					
					var nextBone:VBone = bone.next;
					while (nextBone)
					{
						vecToEndPoint.add(nextBone.direction);
						nextBone = nextBone.next;
					}
					
					var sin:Number = vecToEndPoint.cross(vecToTarget);
					var cos:Number = vecToTarget.dot(vecToEndPoint);
					var ang:Number = Math.atan2(sin, cos);
					
					var angBetween:Number;
					if (bone.prev)
					{
						sin = bone.direction.cross(bone.prev.direction);
						cos = bone.direction.dot(bone.prev.direction);
						angBetween = Math.PI - Math.atan2(sin, cos);
					}
					else
					{
						angBetween = Math.atan2(bone.direction.y, bone.direction.x);
						angBetween += Math.PI
					}
					
					var c:AngleConstraintsValues = _angleConstraints[i];
					if (c)
					{
						var minAllowedAng:Number = c.minAngle - angBetween;
						var maxAllowedAng:Number = c.maxAngle - angBetween;
						
						if (minAllowedAng > ang)
						{
							ang = minAllowedAng;
						}
						
						if (maxAllowedAng < ang)
						{
							ang = maxAllowedAng;
						}
					}
					
					var m:VMatrix2x2 = new VMatrix2x2();
					m.prepareForRotation(ang);
					bone.rotate(m);
				}
				
				bone = _bones[_numBones - 1];
				vecToTarget = target.subtractReturn(bone.position);
				vecToEndPoint = bone.direction.clone();
				var diff:VVector2 = vecToTarget.subtractReturn(vecToEndPoint);
				
				if (diff.squareMagnitude() < 0.01)
				{
					return;
				}
			}
		}
		
		public function render(g:Graphics):void
		{
			for each (var bone:VBone in _bones) 
			{
				bone.render(g);
			}
		}
	}
}